#include <nrk.h>
#include <include.h>
#include <ulib.h>
#include <stdio.h>
#include <avr/sleep.h>
#include <hal.h>
#include <rt_link.h>
#include <nrk_error.h>
#include <nrk_events.h>
#include <nrk_driver.h>
#include <nrk_driver_list.h>
#include <ff_basic_sensor.h>
#include  <math.h>


#define MY_CHANNEL 17
#define MOLE_ID 3



#define MASTER_TX_SLOT  0

#define MY_TX_SLOT ((MOLE_ID*2)+2)


NRK_STK Stack1[NRK_APP_STACKSIZE];
nrk_task_type TaskOne;
void Task1(void);


void nrk_create_taskset();
uint8_t tx_buf[RF_MAX_PAYLOAD_SIZE];
uint8_t rx_buf[RF_MAX_PAYLOAD_SIZE];

nrk_time_t timestart;
nrk_time_t curtime;
uint32_t timeout = 4;


int
main ()
{
  nrk_setup_ports();
  nrk_setup_uart(UART_BAUDRATE_115K2);

  nrk_kprintf( PSTR("Starting up...\r\n") );

  nrk_init();

  nrk_led_clr(0);
  nrk_led_clr(1);
  nrk_led_clr(2);
  nrk_led_clr(3);
  
  nrk_time_set(0,0);
  
  nrk_register_drivers();
  rtl_task_config();
  nrk_create_taskset ();

  nrk_start();
  
  return 0;
}

void Task1()
{
  uint8_t j, i;
  uint8_t rssi, slot,length, mole_id;
  uint8_t *local_rx_buf;
  uint8_t fd,val;
  uint16_t light = 0;
  uint8_t Close = 0;
  uint8_t is_mole_whacked;
  uint16_t threshold = 900;

  printf( "Task1 PID=%d\r\n",nrk_get_pid());
  printf( "Node ID=%d\r\n",MOLE_ID);

  rtl_init ( RTL_MOBILE );
  rtl_set_channel(MY_CHANNEL);
  
  rtl_set_schedule( RTL_RX, MASTER_TX_SLOT, 1 );
  rtl_set_schedule( RTL_TX, MY_TX_SLOT, 1 );
  
  rtl_start();
 
  rtl_rx_pkt_set_buffer(rx_buf, RF_MAX_PAYLOAD_SIZE);
 
  while(!rtl_ready())  nrk_wait_until_next_period(); 
  fd=nrk_open(FIREFLY_SENSOR_BASIC,READ);
  if(fd==NRK_ERROR) nrk_kprintf(PSTR("Failed to open sensor driver\r\n"));

  tx_buf[8] = MOLE_ID + 48;

  val=nrk_set_status(fd,SENSOR_SELECT,LIGHT);
  while(1) {
    if( rtl_rx_pkt_check() == 0 ) {
      rtl_wait_until_rx_or_tx();
    }
    nrk_led_toggle(BLUE_LED);
    local_rx_buf=rtl_rx_pkt_get(&length, &rssi, &slot);
    printf( "RX on slot %d %d: ",slot,length );

    for(i=PKT_DATA_START; i<length; i++ )
      printf( "%c",local_rx_buf[i] );

    if((local_rx_buf[37]-48) == MOLE_ID) {
      nrk_led_set(RED_LED);
      nrk_time_get( &timestart );
      nrk_time_get( &curtime );
      is_mole_whacked = '0';
      while ( curtime.secs - timestart.secs < timeout ){
	val = nrk_read( fd, &light, 2 );
	if ( light > threshold ){
	  is_mole_whacked = '1';
	  break;
	}
	nrk_time_get( &curtime );
      }
      nrk_led_clr(RED_LED);
    }

      printf(" rounds value  %d ", local_rx_buf[51]-48);
      printf("\n NEW: %c",local_rx_buf[37]);
      nrk_kprintf( PSTR("\r\n") );
      rtl_rx_pkt_release();
      nrk_led_toggle(BLUE_LED);

      if ( rtl_tx_pkt_check(MY_TX_SLOT)!=0 ) {
	printf( "Pending on slot %d\r\n",MY_TX_SLOT );
      }
      else{
	  tx_buf[16] = is_mole_whacked;
	  rtl_tx_pkt( tx_buf, RF_MAX_PAYLOAD_SIZE, MY_TX_SLOT );
	  printf("Transmitted packet");
	  nrk_led_toggle(BLUE_LED);
      }
  }
}


void
nrk_create_taskset()
{


  TaskOne.task = Task1;
  TaskOne.Ptos = (void *) &Stack1[NRK_APP_STACKSIZE-1];
  TaskOne.Pbos = (void *) &Stack1[0];
  TaskOne.prio = 2;
  TaskOne.FirstActivation = TRUE;
  TaskOne.Type = BASIC_TASK;
  TaskOne.SchType = PREEMPTIVE;
  TaskOne.period.secs = 6;
  TaskOne.period.nano_secs = 0;
  TaskOne.cpu_reserve.secs = 5;
  TaskOne.cpu_reserve.nano_secs = 100*NANOS_PER_MS;
  TaskOne.offset.secs = 0;
  TaskOne.offset.nano_secs= 0;
  nrk_activate_task (&TaskOne);


  nrk_kprintf( PSTR("Create Done\r\n") );
}


void nrk_register_drivers()
{
  int8_t val;

  // Register the Basic FireFly Sensor device driver
  // Make sure to add: 
  //     #define NRK_MAX_DRIVER_CNT  
  //     in nrk_cfg.h
  // Make sure to add: 
  //     SRC += $(ROOT_DIR)/src/drivers/platform/$(PLATFORM_TYPE)/source/ff_basic_sensor.c
  //     in makefile
  val=nrk_register_driver( &dev_manager_ff_sensors,FIREFLY_SENSOR_BASIC);
  if(val==NRK_ERROR) nrk_kprintf( PSTR("Failed to load my ADC driver\r\n") );

}

